﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_ObjectFinder.cs
 * \brief	This file contains the ObjectFinder function that 
 *  
 * This file contains the ObjectFinder function that 
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Gonzalo Rodriguez \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.03
 * \date      2010-06-23
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio Matta                         2009-11-20 \n
 *      First implementation of the M44_ObjectFinder class.
 *      
 * - Version 0.02:   Gonzalo Rodriguez                      2010-02-03 \n
 *      Change float[] for ArrayList in the open and closed pixel lists
 *      
 *  -Version 0.03: Gonzalo RODRIGUEZ                          2010-06-23 \n
 *      Add the constants that control the whole dynamic object detection algorithm
 *      
/* ######################################################################### */
using System;
using System.Collections.Generic;
using System.Text;

/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Complex;
using Interfaces.Basis;

/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;

/*This is for using lists*/
using System.Collections;

/*Using the C Sharp Matrix Library*/
using NMRS.CSML;

/* M44_SharedClasses */
using NMRS.M44_SharedClasses;

/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_ObjectFinder
     * 
     *  \brief  This is the class of the M44_ObjectFinder function.
     *
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_ObjectFinder
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static DetectedObjects ObjectFinder( float[,] DistanceDiference )
         * 
         *  \brief This functions groups all the cells that belongs to the same object
         *  
         *  \param[in] float[,] DistanceDiference
         *  \param[in] short n_matrix
         *  \param[in] short m_matrix
         *  
         *  \param[out] DetectedObjects str_DetectedObjects
         *
         */
        /* ------------------------------------------------------------------------- */
        public struct Punto
        {
            public int x;
            public int y;
        }
        


        public static DetectedObjects ObjectFinder(float[,] DistanceDiference, short n_matrix, short m_matrix)
        {
            /*Variables*/

            DetectedObjects str_DetectedObjects = new DetectedObjects(n_matrix, m_matrix);
            int[,] ti_Objects = new int[n_matrix, m_matrix];
            int[] u1 = { 0, 1, 1, 1, 0, -1, -1, -1 };
            int[] u2 = { 1, 1, 0, -1, -1, -1, 0, 1 };
            int label = 0; //label for the objects
            int pos_i;
            int pos_j;
            Punto aux_pos = new Punto();
            Punto act_px = new Punto();
            ArrayList ts_OpenPx = new ArrayList();
            ArrayList ts_ClosedPx = new ArrayList();
            ArrayList Objects2eliminate = new ArrayList();
            ArrayList ObjectsLabels = new ArrayList();
            int n_open_px = 0;
            int n_closed_px = 0;
            int n_pixels = 0;
            bool cond1 = false;
            bool cond2 = false;
            int min_pix = DynObj_Const.i_min_number_pixel; 
            
            //Fill the objects matrix with zeros
            for (int i = 0; i < n_matrix; i++)
            {
                for (int j = 0; j < m_matrix; j++)
                {
                    ti_Objects[i,j] = 0;
                }
            }

            //Search for objects
            for (int i = 0; i < n_matrix; i++)
            {
                for (int j = 0; j < m_matrix; j++)
                {
                    //If the difference is not zero (there is a object in this pixel)
                    //There is not a previous object defined for this pixel
                    //We have found a new object
                    if (DistanceDiference[i, j] != 0 && ti_Objects[i, j] == 0)
                    {
                        //reset the list for open and closed pixels
                        n_open_px = 0;
                        n_closed_px = 0;
                        ts_OpenPx = new ArrayList();
                        ts_ClosedPx = new ArrayList();
                        //Surroundings check of the pixel
                        for (int k = 0; k < 8; k++)
                        {
                            pos_i = i + u1[k];
                            pos_j = j + u2[k];
                            //check if it is between boundaries
                            if ((pos_i > -1) && (pos_i < n_matrix) && (pos_j > -1) && (pos_j < m_matrix))
                            {
                                //check if forms part of the object
                                if (DistanceDiference[pos_i,pos_j] != 0)
                                {
                                    //add to the open pixels list
                                    aux_pos.x = pos_i;
                                    aux_pos.y = pos_j;
                                    ts_OpenPx.Add(aux_pos);
                                    n_open_px++;
                                }
                            }
                        }
                        //explore all this object if it has more than one pixel
                        if (n_open_px > 0)
                        {
                            n_pixels = 1;
                            label++;
                            ti_Objects[i, j] = label;
                            while (n_open_px > 0)
                            {

                                act_px = (Punto)ts_OpenPx[0];
                                pos_i = act_px.x;
                                pos_j = act_px.y;

                                if (DistanceDiference[pos_i,pos_j] != 0)  //if the difference is not zero the pixel belongs to the object
                                {
                                    n_pixels++;
                                    ti_Objects[pos_i,pos_j] = label;  //we label the pixel as part of the current object
                                    //look in the surroundings of the pixel in order to introduce more objects in open_px
                                    for (int k = 0; k < 8; k++)
                                    {
                                        pos_i = act_px.x + u1[k];
                                        pos_j = act_px.y + u2[k];
                                        aux_pos.x = pos_i;
                                        aux_pos.y = pos_j;
                                        //find if the pixel between the boundaries
                                        if ((pos_i > -1) && (pos_i < n_matrix) && (pos_j > -1) && (pos_j < m_matrix))
                                        //find if the pixel is in open or closed list
                                        {
                                            cond1 = ts_OpenPx.Contains(aux_pos);
                                            cond2 = ts_ClosedPx.Contains(aux_pos);

                                            if (!cond1 && !cond2) //is not in any list
                                            {
                                                ts_OpenPx.Add(aux_pos);
                                                n_open_px++;
                                            }
                                        }
                                    }
                                }
                                // add pixel to closed List
                                ts_ClosedPx.Add(act_px);
                                ts_OpenPx.RemoveAt(0);
                                n_closed_px++;
                                n_open_px--;
                            }
                            if (n_pixels < min_pix)
                            {
                                Objects2eliminate.Add(label);
                            }
                            else
                            {
                                ObjectsLabels.Add(label);
                            }

                        }
                        

                    }
                }

            }

            for (int k = 0; k < Objects2eliminate.Count; k++)
            {
                for (int i = 0; i < n_matrix; i++)
                {
                    for (int j = 0; j < m_matrix; j++)
                    {
                        if (ti_Objects[i, j] == (int)Objects2eliminate[k])
                            ti_Objects[i, j] = 0;
                    }
                }
                label--;
                
            }

            str_DetectedObjects.ti_Objects = ti_Objects;
            str_DetectedObjects.i_NumberOfObjects = label;
            str_DetectedObjects.s_labels = ObjectsLabels;
            return str_DetectedObjects;
        }

        
    }
}
